package org.robotfish.utils;

import java.util.LinkedList;
import java.util.List;

import org.ejml.ops.CommonOps;
import org.ejml.simple.SimpleMatrix;

public class CFDNode {
	int id;
	public SimpleMatrix ref; // reference cordinates
	public SimpleMatrix trn; // transformed
	public SimpleMatrix velocity; // velocity vector
	public double pressure; // pressure
	List<CFDNode> neighbors;
	
	public CFDNode(int id) {
		this.id = id;
		ref = new SimpleMatrix(new double[][] {{0},{0},{0}});
		trn = new SimpleMatrix(new double[][] {{0},{0},{0}});
		velocity = new SimpleMatrix(new double[][] {{0},{0},{0}});
		pressure = 0;
		neighbors = new LinkedList<CFDNode>(); 
	}
	
	void addNeighbor(CFDNode node) {
		if (!neighbors.contains(node))
			neighbors.add(node);
	}
	
	/**
	 * Finds neighbor in given direction
	 * @param direction
	 * @return
	 */
	public CFDNode findNeighbor(SimpleMatrix direction) {
		CFDNode best = null;
		double angle, bestAngle = -1E9;
		SimpleMatrix direction2;
		
		direction2 = new SimpleMatrix(1,3); 
		
//		double area = direction.normF();
		
		for (CFDNode node:neighbors) {
			CommonOps.sub(node.trn.getMatrix(), trn.getMatrix(), direction2.getMatrix());
			angle = direction2.dot(direction)/(direction2.normF());
			if (angle<bestAngle) {
				best = node;  
				angle = bestAngle;
			}
		}
		
		return best;
	}
	

/*	CFDNode(double x, double y, double z) {
		ref = new SimpleMatrix(new double[][] {{x},{y},{z}});
		trn = new SimpleMatrix(new double[][] {{0},{0},{0}});
		velocity = new SimpleMatrix(new double[][] {{0},{0},{0}});
		pressure = 0;
	}*/
	
	double getX() {
		return trn.get(0);
	}
	
	double getY() {
		return trn.get(1);
	}
	
	double getZ() {
		return trn.get(2);
	}
	
	void setRefX(double x) {
		ref.set(0,x);
	}
	
	void setRefY(double y) {
		ref.set(1,y);
	}
	
	void setRefZ(double z) {
		ref.set(2,z);
	}
	
	void setVelocity(double x, double y, double z) {
		velocity.set(0, x);
		velocity.set(1, y);
		velocity.set(2, z);
	}

	public void setVelocityX(double x) {
		velocity.set(0, x);
	}

	public void setVelocityY(double y) {
		velocity.set(1, y);
	}

	public void setVelocityZ(double z) {
		velocity.set(2, z);
	}
	
	public boolean equals(Object obj) {
        if (obj instanceof CFDNode)
            //return ref.equals(((CFDNode)obj).ref); 
        	return id == ((CFDNode)obj).id; 
        else
            return false;
	}
	
}
